#include <camponormal.h>
#include <auxiliares.h>
#include <fstream>

extern matriz<tripla_dbl>* campo_normal_M;

/* Resolvemos la ecuacion (5) para cada pixel de la imagen */

void obtener_campo_normal(matriz<uint8_t> & m1, matriz<uint8_t> & m2, matriz<uint8_t> & m3)
{
	campo_normal_M = new matriz<tripla_dbl>(filas_imagen, colum_imagen, tripla_dbl(0, 0, 0));	// campo_normal_M esta definida en tp2.cpp
	
	/* Vector direccion de la primera imagen */
	
	vector<double> s1(3, 0);
	
	s1[0] = vectores_S[0].fst();	// X
	s1[1] = vectores_S[0].snd();	// Y
	s1[2] = vectores_S[0].trd();	// Z
		
	/* Vector direccion de la segunda imagen */

	vector<double> s2(3, 0);
	
	s2[0] = vectores_S[1].fst();
	s2[1] = vectores_S[1].snd();
	s2[2] = vectores_S[1].trd();
	
	/* Vector direccion de la tercera imagen */

	vector<double> s3(3, 0);	
	
	s3[0] = vectores_S[2].fst();
	s3[1] = vectores_S[2].snd();
	s3[2] = vectores_S[2].trd();
		
	/* Armamos la matriz del sistema (5) */

	matriz<double> vectS;
	vectS.push_fila(s1);
	vectS.push_fila(s2);
	vectS.push_fila(s3);
	
	/* Archivos de salida */
		
	ofstream normales_x;
	ofstream normales_y;
	ofstream normales_z;
	
	normales_x.open("normales_x.txt");
	normales_y.open("normales_y.txt");
	normales_z.open("normales_z.txt");
		
	for(unsigned int i = 0; i < filas_imagen; i++)
	{
		for(unsigned int j = 0; j < colum_imagen; j++)
		{              
			/* Obtenemos la intensidad del pixel en cada imagen */
			
			vector<double> I(3, 0);
			I[0] = m1[i][j];	
			I[1] = m2[i][j];
			I[2] = m3[i][j];
			
			/* Resolvemos el sistema para obtener los m_x, m_y, m_z */
			
			sistema sis(vectS, I);			
			tripla_dbl res(sis.getIncognita(0), sis.getIncognita(1), sis.getIncognita(2));
			
			/* Ahora normalizamos */
						
			if(esCero(res.fst()) && esCero(res.snd()) && esCero(res.trd()))
			{				
				res = tripla_dbl(0, 0, -1);	// Decidimos poner 1 puesto que sino dividiamos por cero, y son pixeles que pertenecen al fondo y no importa procesarlos	
			}
			else
			{	
				double norma = norma_2(res);
				
				if(esCero(norma))
					cout << "obtener_campo_normal: Una norma da cero!" << endl;
					
				res = tripla_dbl(res.fst() / norma, res.snd() / norma, res.trd() / norma);		// Normalizamos el vector Io * p = ||m||
			}
			
			(*campo_normal_M)[i][j] = res;		// Guardamos la tripla en la matriz
			
			normales_x << res.fst() << " ";
			normales_y << res.snd() << " ";
			normales_z << res.trd() << " ";			
		}
		
		normales_x << endl;
		normales_y << endl;
		normales_z << endl;
	}
	
	normales_x.close();
	normales_y.close();
	normales_z.close();
}
	

